/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CCornerMapMCL.h"

CCornerMapMCL::CCornerMapMCL(){
		
		num_of_meas = 0;
		sensor_offset = 0;
}
CCornerMapMCL::~CCornerMapMCL(){
}

/**
 * Initializes the filter - uses normal random method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p num_of_particles The number of particles in the filter
 * @p init_pose the initial pose
 * @p initVariance the variance for the initial quess
 * @p num_of_meas_in_scan number of measurements used from the scan
 */
void CCornerMapMCL::initialize(const char *mapfile,
								float mapscale, 
								int num_of_particles, 
								mcl::pose initPose, 
								mcl::pose initVariance,
								int num_of_meas_in_scan,
								float _sensor_offset){

		fprintf(stderr,"CLineMapRawScanMCL::initialize(): Called with num_of_meas_in_scan=%d\n",num_of_meas_in_scan);
		
		num_of_meas  =	num_of_meas_in_scan;
		sensor_offset = _sensor_offset;
		
		filt.initializeNormalRandom(initPose, initVariance,num_of_particles);													
}
/**
 * Same as init but without mallocs
 */
void CCornerMapMCL::reinitializeNR(mcl::pose initPose,mcl::pose initVariance){
		filt.initializeNormalRandom(initPose, initVariance,filt.NumOfParticles);	
}

/**
 * Run the MCL step
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void CCornerMapMCL::runMCL(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		int fac;
		mcl::pose var;
		
		fac = measurement.N / num_of_meas; ///< The ratio between the measurement and wanted num_of_meas
		
		mcl_meas.allocate(num_of_meas);
		mcl_meas.N=num_of_meas;
		
		///Take the measurements that will be used in estimation
		/// The number is reduced for computational & efficiency reasons i.e. MCL must not be "too accurate"
		for(int i=0;i<num_of_meas;i++){
			//fprintf(stderr,"i=%d faci=%d ",i,fac*i);
				mcl_meas.r[i] = measurement.r[fac*i];	
				mcl_meas.a[i] = measurement.a[fac*i];
			if(fac*i>=measurement.N) 
					fprintf(stderr,"runMCL:: Terrible mistake - expect crash!\n");
		}
		
		filt.predict(dP,variance); ///<Predict
		
		calculateLikelihood(mcl_meas); ///<Calculate likelihoods
		
		filt.normalize(); 
		var = filt.getDistributionVariances(); 
		
		///Using now stetson harrison method
		if(var.x > .5 || var.y>.5 || var.a > (M_PI/8)){
				fprintf(stderr,"Performing SIR \n");
				filt.SIRUpdate();
		}
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}

/**
 * Calculates the likelihood of particle distribution
 * uses the CParticleFilter filt and TLineMap map
 * @param meas The current measurement
 * Uses the private members:
				float *virtualr;
				float *virtuallines;
				float *virtualscan_xy;
				float *virtualscan_xy_rel;
				int num_of_meas;
 **/
void CCornerMapMCL::calculateLikelihood(mcl::scan meas)
{
		int i,j;
		
		float dL2,dl;
		float Lik;
		float Liks;
		int outLiers;

		filt.Lik = 0;
		filt.outLiers = 0;
		///Cycle through each particle
		//fprintf(stderr,"Numofmeas=%d, num particles=%d\n",num_of_meas,filt.NumOfParticles);
		
		for(i=0;i<filt.NumOfParticles;i++){		
				outLiers = 0;
				
				filt.outLiers+=(float) outLiers;
		
				Liks = 1;
		
				for(j=0;j<num_of_meas;j++){	
						//r[j] = sqrt( Scan[2*j]* Scan[2*j] + Scan[2*j+1]*Scan[2*j+1]);
						//r_virt = sqrt(virtualScan[2*j]*virtualScan[2*j] + virtualScan[2*j+1]*virtualScan[2*j+1]);
				
						dl = 0;
						//If the value was ok
						if(meas.r[j]>=0){
				
								if(dl < -1.5f){
										Lik = 0.4f;
										Liks = Liks*Lik;
								}
								else{
										dL2 = dl*dl;
										Lik = (1.0- FALSE_PROB) * exp(  - (dL2/(VMEAS)) );
										//fprintf(stderr,"%.2f ",Lik);
										Lik += FALSE_PROB;
										Liks = Liks*Lik;
								}
						}
				}
				filt.Lik+=Liks;
				filt.Particles[i].lik=Liks;
		}
		filt.outLiers = (filt.outLiers / ( num_of_meas * filt.NumOfParticles))*100;

}
